/*  -------------------------- (C) COPYRIGHT 2020 Fortiortech ShenZhen ---------------------------*/
/*  File Name      : ControlLoop.c
    /*  Author         : Fortiortech  Appliction Team
    /*  Version        : V1.0
    /*  Date           : 2021-03-21
    /*  Description    : 电机控制环路
    /*  ----------------------------------------------------------------------------------------------*/
/*                                     All Rights Reserved
    /*  ----------------------------------------------------------------------------------------------*/

#include "MyProject.h"

//uint16 xdata ControlLoopOut;
uint16 speed1 = 0;
int16  SpeedDEC = 0;
/*  -------------------------------------------------------------------------------------------------
    Function Name  : HW_Speed_PI
    Description    : PI控制
    Date           : 2020-04-10
    Parameter      : Xn1: [输入/出]
    ------------------------------------------------------------------------------------------------- */
int16 HW_Speed_PI(int16 Xn1)
{
    PI3_EK = Xn1;          //填入EK
    SetBit(PI_CR, PI3STA); // Start PI
    
    while (ReadBit(PI_CR, PIBSY))
        ;
        
    return PI3_UKH;
}
/*  -------------------------------------------------------------------------------------------------
    Function Name  : ControlLoopInit
    Description    : 速度环初始化
    Date           : 2020-04-10
    Parameter      : Xn1: [输入/出]
    ------------------------------------------------------------------------------------------------- */
void ControlLoopInit(void)
{
    //速度环相关参数初始化
    static uint8 Old_boost = 5;
    MotorSpeed.IncValue = SPEED_INC;
    MotorSpeed.DecValue = SPEED_DEC;
    MotorSpeed.SpeedScalerH = (uint16)(SPEEDSCALER >> 16);
    MotorSpeed.SpeedScalerL = (uint16)(SPEEDSCALER & 0x0000FFFF);
    MotorSpeed.LoopFlag = 0;
       
    PI3_Init(); // 速度环PI参数初始化
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : mc_ramp
    Description    :
    Date           : 2020-04-10
    Parameter      : Speedramp: [输入/出]
    ------------------------------------------------------------------------------------------------- */
void mc_ramp(void)
{
	      if (MotorSpeed.ActualValue + MotorSpeed.IncValue < MotorSpeed.TargetValue)
        {
            MotorSpeed.ActualValue += MotorSpeed.IncValue;
        }
        else if (MotorSpeed.ActualValue - MotorSpeed.DecValue > MotorSpeed.TargetValue)
        {
            MotorSpeed.ActualValue -= MotorSpeed.DecValue;
        }
        else
        {
            MotorSpeed.ActualValue = MotorSpeed.TargetValue;
        }
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : Duty_Function()
    Description    :
    Date           : 2020-04-10
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */

void Duty_Function(void)
{
    /*启动阶段进行DUTY的调整*/
    if(BLDC.BLDCSetFlag == 1)
		{
    	if(BLDC.COM_COUNT==1)
			DRV_DR = BLDC_Start_Duty0;
        
		else if(BLDC.COM_COUNT==2)
			DRV_DR = BLDC_Start_Duty1;
        
		else if (BLDC.COM_COUNT==3)
			DRV_DR = BLDC_Start_Duty2;
        
		else if (BLDC.COM_COUNT==4)
			DRV_DR = BLDC_Start_Duty3;
        
		if (DRV_DR < MAX_START_Duty)
			DRV_DR = DRV_DR + 1;

    }
	  else if (BLDC.BLDCSetFlag == 2)
    {
        MotorSpeed.DelayPeriod++;
        
		if (MotorSpeed.DelayPeriod > (SPEED_LOOP_TIME * PWM_FREQUENCY)) // 2ms进行一次pi计算
		{
			MotorSpeed.DelayPeriod = 0;
			MotorSpeed.SpeedBCOR = TIM1_BCOR;
			DivQ_L_MDU(MotorSpeed.SpeedScalerH, MotorSpeed.SpeedScalerL, MotorSpeed.SpeedBCOR, MotorSpeed.SpeedFltBCOR);
			
			if (MotorSpeed.LoopFlag == 0)
			{
				MotorSpeed.LoopFlag = 1;
				speed1 = DRV_DR;
				PI3_UKH = speed1;                                 //第一次进入速度环，PI3_UKH赋值防止发生超调
				MotorSpeed.ActualValue = MotorSpeed.SpeedFltBCOR;
			}
			
			mc_ramp();

			SpeedDEC = MotorSpeed.ActualValue - MotorSpeed.SpeedFltBCOR;
			speed1 = HW_Speed_PI(SpeedDEC);
		  
			if (speed1 > (DRV_DR + 1))
			{
				DRV_DR = DRV_DR + 1;
			}
			else if (speed1 < (DRV_DR - 1))
			{
				DRV_DR = DRV_DR - 1;
			}
			else
			{
				DRV_DR = speed1;
			}
		}
    }
}
